
#ifndef __TMATH_H__
#define	__TMATH_H__

#include "STLTypes.h"

#define	_PI			3.1415926535897932384626433832795
#define _Radius		0.01745329251994329576923690768489
#define	_Degree		57.295779513082320876798154814105


template <class T>
class TVector2
{
public:
	TVector2()
		: x(0), y(0)
	{}

	TVector2(T const& a, T const& b)
		: x(a), y(b)
	{}

	TVector2(T const& v)
		: x(v.x), y(v.y)
	{}

	void set(T const& a, T const& b)
	{
		x = a;	y = b;
	}

	void set(TVector2<T> const& v)
	{
		x = v.x;	y = v.y;
	}

	operator T* () { return &x; }

	operator const T* () const { return &x; }

protected:

	T x, y;
};

template <class T>
class TVector3
{
public:
	TVector3()
		: x(0), y(0), z(0)
	{}

	TVector3(T const& a, T const& b, T const& c)
		: x(a), y(b), z(c)
	{}

	TVector3(T const& v)
		: x(v.x), y(v.y), z(v.z)
	{}

	void set(T const& a, T const& b, T const& c)
	{
		x = a;	y = b;	z = c;
	}

	void set(TVector3<T> const& v)
	{
		x = v.x;	y = v.y;	z = v.z;
	}

	operator T* () { return &x; }

	operator const T* () const { return &x; }

protected:

	T x, y, z;
};

template <class T>
class TVector4
{
public:
	TVector4()
		: x(0), y(0), z(0), w(0)
	{}

	TVector4(T const& a, T const& b, T const& c, T const& d)
		: x(a), y(b), z(c), w(d)
	{}

	TVector4(TVector4<T> const& v)
		: x(v.x), y(v.y), z(v.z), w(v.w)
	{}

	void set(T const& a, T const& b, T const& c, T const& d)
	{
		x = a;	y = b;	z = c;	w = d;
	}

	void set(TVector4<T> const& v)
	{
		x = v.x;	y = v.y;	z = v.z;	w = v.w;
	}

	operator T* () { return &x; }

	operator const T* () const { return &x; }

protected:

	T x, y, z, w;
};

template <class T>
class TQuaternion
{
public:
	TQuaternion()
	{}

	TQuaternion(T a, T b, T c, T d)
		: x(a), y(b), z(c), w(d)
	{}

protected:
	T x, y, z, w;

};

typedef	TQuaternion<float>		Quaternionf;
typedef	TQuaternion<double>		Quaterniond;

template <class T>
class TMatrix4
{
public:
	TMatrix4()
	{
		identity();
	}

	operator T* () { return colum[0]; }

	operator const T* () const { return colum[0]; }

	//operator TVector4<T>* () { return colum; }

	//operator const TVector4<T>*() const { return colum; }

	void identity(void)
	{
		colum[0].set(1, 0, 0, 0);
		colum[1].set(0, 1, 0, 0);
		colum[2].set(0, 0, 1, 0);
		colum[3].set(0, 0, 0, 1);
	}

	void translate(TVector3<T> const& v)
	{
		colum[0].set(1, 0, 0, 0);
		colum[1].set(0, 1, 0, 0);
		colum[2].set(0, 0, 1, 0);
		colum[3].set(v[0], v[1], v[2], 1);
	}

	void frustum(T const& left, T const& right, T const& bottom, T const& top, T const& nearval, T const& farval)
	{
		T x, y, a, b, c, d;

		x = (T(2.0)*nearval) / (right - left);
		y = (T(2.0)*nearval) / (top - bottom);
		a = (right + left) / (right - left);
		b = (top + bottom) / (top - bottom);
		c = -(farval + nearval) / (farval - nearval);
		d = -(T(2.0)*farval*nearval) / (farval - nearval);  /* error? */

		colum[0][0] = x;	colum[1][0] = 0;	colum[2][0] = a;		colum[3][0] = 0;
		colum[0][1] = 0;	colum[1][1] = y;	colum[2][1] = b;		colum[3][1] = 0;
		colum[0][2] = 0;	colum[1][2] = 0;	colum[2][2] = c;		colum[3][2] = d;
		colum[0][3] = 0;	colum[1][3] = 0;	colum[2][3] = T(-1.0);	colum[3][3] = 0;
	}

	void perspective(T const& fovRadius, T const& aspectRatio, T const& zNear, T const& zFar)
	{
		T zNearTan = zNear * (T)tan(fovRadius / T(2.0));
		T zNearRatio = zNearTan * aspectRatio;
		frustum(-zNearRatio, zNearRatio, -zNearTan, zNearTan, zNear, zFar);
	}

	void rotate(T const& angle, TVector3<T> const& axis)
	{
		T xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c, s, c;
		bool optimized;

		T x = axis[0], y = axis[1], z = axis[2];

		s = (T)sin(angle);
		c = (T)cos(angle);

		optimized = false;

		if (x == T(0.0)) {
			if (y == T(0.0)) {
				if (z != T(0.0)) {
					optimized = true;
					/* rotate only around z-axis */
					colum[0][0] = c;
					colum[1][1] = c;
					if (z < T(0.0)) {
						colum[0][1] = -s;
						colum[1][0] = s;
					}
					else {
						colum[0][1] = s;
						colum[1][0] = -s;
					}
				}
			}
			else if (z == T(0.0)) {
				optimized = true;
				/* rotate only around y-axis */
				colum[0][0] = c;
				colum[2][2] = c;
				if (y < T(0.0)) {
					colum[0][2] = s;
					colum[2][0] = -s;
				}
				else {
					colum[0][2] = -s;
					colum[2][0] = s;
				}
			}
		}
		else if (y == T(0.0)) {
			if (z == T(0.0)) {
				optimized = true;
				/* rotate only around x-axis */
				colum[1][1] = c;
				colum[2][2] = c;
				if (x < T(0.0)) {
					colum[1][2] = -s;
					colum[2][1] = s;
				}
				else {
					colum[1][2] = s;
					colum[2][1] = -s;
				}
			}
		}

		if (!optimized) {
			const T mag = sqrtf(x * x + y * y + z * z);

			if (mag <= 1.0e-4) {
				/* no rotation, leave mat as-is */
				return;
			}

			x /= mag;
			y /= mag;
			z /= mag;


			/*
			*     Arbitrary axis rotation matrix.
			*
			*  This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied
			*  like so:  Rz * Ry * T * Ry' * Rz'.  T is the final rotation
			*  (which is about the X-axis), and the two composite transforms
			*  Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary
			*  from the arbitrary axis to the X-axis then back.  They are
			*  all elementary rotations.
			*
			*  Rz' is a rotation about the Z-axis, to bring the axis vector
			*  into the x-z plane.  Then Ry' is applied, rotating about the
			*  Y-axis to bring the axis vector parallel with the X-axis.  The
			*  rotation about the X-axis is then performed.  Ry and Rz are
			*  simply the respective inverse transforms to bring the arbitrary
			*  axis back to its original orientation.  The first transforms
			*  Rz' and Ry' are considered inverses, since the data from the
			*  arbitrary axis gives you info on how to get to it, not how
			*  to get away from it, and an inverse must be applied.
			*
			*  The basic calculation used is to recognize that the arbitrary
			*  axis vector (x, y, z), since it is of unit length, actually
			*  represents the sines and cosines of the angles to rotate the
			*  X-axis to the same orientation, with theta being the angle about
			*  Z and phi the angle about Y (in the order described above)
			*  as follows:
			*
			*  cos ( theta ) = x / sqrt ( 1 - z^2 )
			*  sin ( theta ) = y / sqrt ( 1 - z^2 )
			*
			*  cos ( phi ) = sqrt ( 1 - z^2 )
			*  sin ( phi ) = z
			*
			*  Note that cos ( phi ) can further be inserted to the above
			*  formulas:
			*
			*  cos ( theta ) = x / cos ( phi )
			*  sin ( theta ) = y / sin ( phi )
			*
			*  ...etc.  Because of those relations and the standard trigonometric
			*  relations, it is pssible to reduce the transforms down to what
			*  is used below.  It may be that any primary axis chosen will give the
			*  same results (modulo a sign convention) using thie method.
			*
			*  Particularly nice is to notice that all divisions that might
			*  have caused trouble when parallel to certain planes or
			*  axis go away with care paid to reducing the expressions.
			*  After checking, it does perform correctly under all cases, since
			*  in all the cases of division where the denominator would have
			*  been zero, the numerator would have been zero as well, giving
			*  the expected result.
			*/

			xx = x * x;
			yy = y * y;
			zz = z * z;
			xy = x * y;
			yz = y * z;
			zx = z * x;
			xs = x * s;
			ys = y * s;
			zs = z * s;
			one_c = T(1.0) - c;

			/* We already hold the identity-matrix so we can skip some statements */
			colum[0][0] = (one_c * xx) + c;
			colum[0][1] = (one_c * xy) + zs;
			colum[0][2] = (one_c * zx) - ys;
			/*	colum[0][3] = T(0.0); */

			colum[1][0] = (one_c * xy) - zs;
			colum[1][1] = (one_c * yy) + c;
			colum[1][2] = (one_c * yz) + xs;
			/*	colum[1][3] = T(0.0); */

			colum[2][0] = (one_c * zx) + ys;
			colum[2][1] = (one_c * yz) - xs;
			colum[2][2] = (one_c * zz) + c;
			/*	colum[2][3] = T(0.0); */

			/*
			colum[3][0] = T(0.0);
			colum[3][1] = T(0.0);
			colum[3][2] = T(0.0);
			colum[3][3] = T(1.0);
			*/
		}
	}

	TMatrix4<T> operator * (TMatrix4<T> const& m) const
	{
		TMatrix4<T> mat;

		for (int i = 0; i < 4; i++) {
			mat.colum[0][i] = colum[0][i] * m.colum[0][0] + colum[1][i] * m.colum[0][1] + colum[2][i] * m.colum[0][2] + colum[3][i] * m.colum[0][3];
			mat.colum[0][i] = colum[0][i] * m.colum[0][0] + colum[1][i] * m.colum[0][1] + colum[2][i] * m.colum[0][2] + colum[3][i] * m.colum[0][3];
			mat.colum[1][i] = colum[0][i] * m.colum[1][0] + colum[1][i] * m.colum[1][1] + colum[2][i] * m.colum[1][2] + colum[3][i] * m.colum[1][3];
			mat.colum[2][i] = colum[0][i] * m.colum[2][0] + colum[1][i] * m.colum[2][1] + colum[2][i] * m.colum[2][2] + colum[3][i] * m.colum[2][3];
			mat.colum[3][i] = colum[0][i] * m.colum[3][0] + colum[1][i] * m.colum[3][1] + colum[2][i] * m.colum[3][2] + colum[3][i] * m.colum[3][3];
		}

		return mat;
	}

protected:

	TVector4<T>	colum[4];
};


typedef	TVector3<float>		Vector3f;
typedef	TVector4<float>		Vector4f;

typedef	TVector4<uint32>		Vector4ui;

typedef	TMatrix4<float>		Matrix4f;

#endif // __TMATH_H__
